/*****************************************************
  Mars explorer NXT code in Ch
  University of Leicester GReP 2011
  Using http://iel.ucdavis.edu/projects/chmindstorms/
 *****************************************************/
/*****************************************************
  test.ch 
  
  A simple program to check that the robot can move
  correctly.
 *****************************************************/
 
#include <conio.h>
#include <stdio.h>
#include <iostream.h>

#include "control.h"

int main()
{
  int numpoints = 40;
  double angle[numpoints]; //this stores the actual angle moved as reported from the tachometer
	int distance[numpoints]; //this stores the data received from the ultrasonic sensor  
  
  setup();
  Sensor ultra = Sensor(PORT_4, ULTRASONIC, RAWMODE);
  Sensor compass = Sensor(PORT_3, ULTRASONIC, RAWMODE);
  
  cout << "> Move 20cm forwards" << endl;
  moveStep(20, 20);
  cout << "> Move 20cm backwards" << endl;
  moveStep(-20, 20);
  cout << "> Rotate 360 degrees clockwise" << endl;
  rotateStep(90, 20);
  cout << "> Rotate 360 degrees anti-clockwise" << endl;
  rotateStep(-90, 20);
  cout << "> Scanning using ultrasound sensor" << endl;
  testTurret();

  disconnect(); 
  
  return 0;
}
